#include <iostream>
#include <fstream>
#include <math.h>
#include <stdio.h>

#include <rw/rw.hpp>

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

void printTransformation(rw::math::Transform3D<double>& t);
cv::Mat skew(cv::Mat m);
void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij);

int main()
{
	int cornerRow = 6;
	int cornerColum = 9;
	double patternLength = 30.0; //mm

	vector<rw::math::Transform3D<double> > chessboardPose;

	vector<Point3d> objectPoints;
	int pointNumber = cornerRow * cornerColum;
	for (int i = 0; i < cornerRow; i++)
	{
		for (int j = 0; j < cornerColum; j++)
		{
			Point3d p;
			p.x = patternLength * j;
			p.y = patternLength * i;
			p.z = 0;
			objectPoints.push_back(p);
		}
	}

	//camera intrinsic parameters from "\workcell_scene\arvp\cameras\cameras.rough.wc.xml"
	//<Property name="Camera">31.0482 960 720</Property>
	int imageResolutionX = 960;
	int imageResolutionY = 720;
	double fovy = 31.0482 * 3.14159265 / 180;
	double focalLength = (imageResolutionY / 2.0) / (tan(fovy / 2.0));

	Mat cameraMatrix = Mat::zeros(3, 3, CV_64FC1);
	cameraMatrix.at<double>(0, 0) = focalLength;
	cameraMatrix.at<double>(0, 2) = imageResolutionX / 2.0f;
	cameraMatrix.at<double>(1, 1) = focalLength;
	cameraMatrix.at<double>(1, 2) = imageResolutionY / 2.0f;
	cameraMatrix.at<double>(2, 2) = 1.0f;

	Mat distortionCoff = Mat::zeros(1, 4, CV_64FC1);
	cout << "\nCamera Matrix: \n" << cameraMatrix << endl;
	cout << "\ndistortionCoff: \n" << distortionCoff << endl;

	//namedWindow("Display Image", WINDOW_AUTOSIZE);

	int imageNumber = 25;

	for (int i = 0; i < imageNumber; i++)
	{
		char imageName[150];
		sprintf(imageName, "test-data-01\\image%05d.bmp", i);
		//char* imageName = "test-data-01\\image00000.bmp";

		Mat imageColored = imread(imageName, 1);
		if (!imageColored.data)
		{
			printf("No image data \n");
			return -1;
		}
		Mat image;
		cv::cvtColor(imageColored, image, CV_BGR2GRAY);

		Size patternSize(cornerColum, cornerRow);
		vector<Point2f> corners;

		bool patternfound = findChessboardCorners(image, patternSize, corners, CALIB_CB_ADAPTIVE_THRESH);

		if (patternfound)
		{
			printf("found chessboard of image%05d!\n", i);
			cornerSubPix(image, corners, Size(11, 11), Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
		}

		//drawChessboardCorners(imageColored, patternSize, Mat(corners), patternfound);
		//imshow("Display Image", imageColored);
		//imwrite("test.png", imageColored);
		//waitKey(0);

		Mat rvec; //reference to camera frame
		Mat tvec; //reference to camera frame
		solvePnP(objectPoints, corners, cameraMatrix, distortionCoff, rvec, tvec);

		Mat chessbaordRotationMatrix; //reference to camera frame
		Rodrigues(rvec, chessbaordRotationMatrix);

		rw::math::Transform3D<double> t;
		t(0, 0) = chessbaordRotationMatrix.at<double>(0, 0);
		t(0, 1) = chessbaordRotationMatrix.at<double>(0, 1);
		t(0, 2) = chessbaordRotationMatrix.at<double>(0, 2);
		t(0, 3) = tvec.at<double>(0) / 1000; //mm to m
		t(1, 0) = chessbaordRotationMatrix.at<double>(1, 0);
		t(1, 1) = chessbaordRotationMatrix.at<double>(1, 1);
		t(1, 2) = chessbaordRotationMatrix.at<double>(1, 2);
		t(1, 3) = tvec.at<double>(1) / 1000; //mm to m
		t(2, 0) = chessbaordRotationMatrix.at<double>(2, 0);
		t(2, 1) = chessbaordRotationMatrix.at<double>(2, 1);
		t(2, 2) = chessbaordRotationMatrix.at<double>(2, 2);
		t(2, 3) = tvec.at<double>(2) / 1000; //mm to m

		//printTransformation(t);

		chessboardPose.push_back(t);
	}

	FILE * pFile;
	pFile = fopen("test-data-01\\chessboardPose.txt", "w");
	int size = chessboardPose.size();
	for (int i = 0; i < size; i++)
	{
		rw::math::Transform3D<double> t = chessboardPose.at(i);

		fprintf(pFile, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", t(0, 0), t(0, 1), t(0, 2), t(0, 3),
			t(1, 0), t(1, 1), t(1, 2), t(1, 3),
			t(2, 0), t(2, 1), t(2, 2), t(2, 3));
	}
	fclose(pFile);

	fstream file;
	string filePath = "test-data-01\\myfile.txt";
	file.open(filePath, ios::in);
	if (!file.is_open())
	{
		cout << "Can not find " << filePath << endl;
		return -1;
	}

	string buff;
	int i = 0;
	vector <vector<double> > vectorDst;
	while (getline(file, buff))
	{
		vector<double> nums;
		char *inputString = (char *)buff.c_str();
		const char *split = ",";
		char *next;

		char *p = strtok_s(inputString, split, &next);
		double a;
		while (p != NULL)
		{
			a = atof(p);
			nums.push_back(a);
			p = strtok_s(NULL, split, &next);
		}
		vectorDst.push_back(nums);
		i++;
	}
	file.close();

	//for (int i = 0; i < vectorDst.size(); i++)
	//{
	//	for (int j = 0; j < 12; j++)
	//	{
	//		cout << vectorDst.at(i).at(j) << "\t";
	//	}
	//	cout << endl;
	//}

	vector<rw::math::Transform3D<double> > tcpPose;
	
	for (int i = 0; i < vectorDst.size(); i++) 
	{
		rw::math::Transform3D<double> t;
		t(0, 0) = vectorDst.at(i).at(0);
		t(0, 1) = vectorDst.at(i).at(1);
		t(0, 2) = vectorDst.at(i).at(2);
		t(0, 3) = vectorDst.at(i).at(3);
		t(1, 0) = vectorDst.at(i).at(4);
		t(1, 1) = vectorDst.at(i).at(5);
		t(1, 2) = vectorDst.at(i).at(6);
		t(1, 3) = vectorDst.at(i).at(7);
		t(2, 0) = vectorDst.at(i).at(8);
		t(2, 1) = vectorDst.at(i).at(9);
		t(2, 2) = vectorDst.at(i).at(10);
		t(2, 3) = vectorDst.at(i).at(11);

		tcpPose.push_back(t);
	}

	int count = floor(tcpPose.size()/2.0);
	vector<Mat> w2e;
	vector<Mat> c2g;

	for (int i = 0; i < count; i++)
	{
		rw::math::Transform3D<double> t1 =  rw::math::inverse(tcpPose.at(i * 2 + 1)) * tcpPose.at(i * 2);
		rw::math::Transform3D<double> t2 = chessboardPose.at(i * 2 + 1) * rw::math::inverse(chessboardPose.at(i * 2));
		
		//change the order, the calibration result is nearly same
		//rw::math::Transform3D<double> t1 = rw::math::inverse(tcpPose.at(i * 2)) * tcpPose.at(i * 2 + 1);
		//rw::math::Transform3D<double> t2 = chessboardPose.at(i * 2) * rw::math::inverse(chessboardPose.at(i * 2 + 1));

		Mat tempw2e = Mat(4, 4, CV_64FC1, 0.0f);
		tempw2e.at<double>(0, 0) = t1(0, 0);
		tempw2e.at<double>(0, 1) = t1(0, 1);
		tempw2e.at<double>(0, 2) = t1(0, 2);
		tempw2e.at<double>(0, 3) = t1(0, 3);
		tempw2e.at<double>(1, 0) = t1(1, 0);
		tempw2e.at<double>(1, 1) = t1(1, 1);
		tempw2e.at<double>(1, 2) = t1(1, 2);
		tempw2e.at<double>(1, 3) = t1(1, 3);
		tempw2e.at<double>(2, 0) = t1(2, 0);
		tempw2e.at<double>(2, 1) = t1(2, 1);
		tempw2e.at<double>(2, 2) = t1(2, 2);
		tempw2e.at<double>(2, 3) = t1(2, 3);
		tempw2e.at<double>(3, 0) = 0;
		tempw2e.at<double>(3, 1) = 0;
		tempw2e.at<double>(3, 2) = 0;
		tempw2e.at<double>(3, 3) = 1;

		w2e.push_back(tempw2e);
		
		Mat tempc2g = Mat(4, 4, CV_64FC1, 0.0f);
		tempc2g.at<double>(0, 0) = t2(0, 0);
		tempc2g.at<double>(0, 1) = t2(0, 1);
		tempc2g.at<double>(0, 2) = t2(0, 2);
		tempc2g.at<double>(0, 3) = t2(0, 3);
		tempc2g.at<double>(1, 0) = t2(1, 0);
		tempc2g.at<double>(1, 1) = t2(1, 1);
		tempc2g.at<double>(1, 2) = t2(1, 2);
		tempc2g.at<double>(1, 3) = t2(1, 3);
		tempc2g.at<double>(2, 0) = t2(2, 0);
		tempc2g.at<double>(2, 1) = t2(2, 1);
		tempc2g.at<double>(2, 2) = t2(2, 2);
		tempc2g.at<double>(2, 3) = t2(2, 3);
		tempc2g.at<double>(3, 0) = 0;
		tempc2g.at<double>(3, 1) = 0;
		tempc2g.at<double>(3, 2) = 0;
		tempc2g.at<double>(3, 3) = 1;

		c2g.push_back(tempc2g);
	}

	Mat e2c = Mat(4, 4, CV_64FC1, 0.0f);
	Tsai_HandEye(e2c, w2e, c2g);

	cout << "\nHand in eye calibration results:\n" << e2c << endl;

	//camera extrinsic parameters from "\workcell_scene\arvp\cameras\cameras.rough.wc.xml"
	//<Frame name="Hand-Eye" refframe="UR.TCP">
	//  <RPY> 0 -15 0 </RPY >
	//	<Pos> 0.1 0 0 </Pos >
	//</Frame>
	rw::math::RPY<double> rpy(0, -15*rw::math::Deg2Rad, 0);
	rw::math::Transform3D<double> idealCamera2TCP(rw::math::Vector3D<double>(0.1, 0, 0), rpy.toRotation3D());
	
	cout << "\nIdeal Camera frame refer to robot TCP:" << endl;
	printTransformation(idealCamera2TCP);

	return 0;
}

void printTransformation(rw::math::Transform3D<double>& t)
{
	printf("%15.12f,%15.12f,%15.12f,%15.12f\n%15.12f,%15.12f,%15.12f,%15.12f\n%15.12f,%15.12f,%15.12f,%15.12f\n", t(0, 0), t(0, 1), t(0, 2), t(0, 3),
		t(1, 0), t(1, 1), t(1, 2), t(1, 3),
		t(2, 0), t(2, 1), t(2, 2), t(2, 3));
}

cv::Mat skew(cv::Mat m)
{
	Mat t(3, 3, CV_64FC1);
	t.at<double>(0, 0) = 0;
	t.at<double>(0, 1) = - m.at<double>(2, 0);
	t.at<double>(0, 2) = m.at<double>(1, 0);
	t.at<double>(1, 0) = m.at<double>(2, 0);
	t.at<double>(1, 1) = 0;
	t.at<double>(1, 2) = - m.at<double>(0, 0);
	t.at<double>(2, 0) = - m.at<double>(1, 0);
	t.at<double>(2, 1) = m.at<double>(0, 0);
	t.at<double>(2, 2) = 0;

	return t;
}

void Tsai_HandEye(Mat Hcg, vector<Mat> Hgij, vector<Mat> Hcij)
{
	CV_Assert(Hgij.size() == Hcij.size());
	int nStatus = Hgij.size();

	Mat Rgij(3, 3, CV_64FC1);
	Mat Rcij(3, 3, CV_64FC1);

	Mat rgij(3, 1, CV_64FC1);
	Mat rcij(3, 1, CV_64FC1);

	double theta_gij;
	double theta_cij;

	Mat rngij(3, 1, CV_64FC1);
	Mat rncij(3, 1, CV_64FC1);

	Mat Pgij(3, 1, CV_64FC1);
	Mat Pcij(3, 1, CV_64FC1);

	Mat tempA(3, 3, CV_64FC1);
	Mat tempb(3, 1, CV_64FC1);

	Mat A;
	Mat b;
	Mat pinA;

	Mat Pcg_prime(3, 1, CV_64FC1);
	Mat Pcg(3, 1, CV_64FC1);
	Mat PcgTrs(1, 3, CV_64FC1);

	Mat Rcg(3, 3, CV_64FC1);
	Mat eyeM = Mat::eye(3, 3, CV_64FC1);

	Mat Tgij(3, 1, CV_64FC1);
	Mat Tcij(3, 1, CV_64FC1);

	Mat tempAA(3, 3, CV_64FC1);
	Mat tempbb(3, 1, CV_64FC1);

	Mat AA;
	Mat bb;
	Mat pinAA;

	Mat Tcg(3, 1, CV_64FC1);

	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);

		Rodrigues(Rgij, rgij);
		Rodrigues(Rcij, rcij);

		theta_gij = norm(rgij);
		theta_cij = norm(rcij);

		rngij = rgij / theta_gij;
		rncij = rcij / theta_cij;

		Pgij = 2 * sin(theta_gij / 2)*rngij;
		Pcij = 2 * sin(theta_cij / 2)*rncij;

		tempA = skew(Pgij + Pcij);
		tempb = Pcij - Pgij;

		A.push_back(tempA);
		b.push_back(tempb);
	}

	//Compute rotation
	invert(A, pinA, DECOMP_SVD);

	Pcg_prime = pinA * b;
	Pcg = 2 * Pcg_prime / sqrt(1 + norm(Pcg_prime) * norm(Pcg_prime));
	PcgTrs = Pcg.t();
	Rcg = (1 - norm(Pcg) * norm(Pcg) / 2) * eyeM + 0.5 * (Pcg * PcgTrs + sqrt(4 - norm(Pcg)*norm(Pcg))*skew(Pcg));

	//Computer Translation 
	for (int i = 0; i < nStatus; i++)
	{
		Hgij[i](Rect(0, 0, 3, 3)).copyTo(Rgij);
		Hcij[i](Rect(0, 0, 3, 3)).copyTo(Rcij);
		Hgij[i](Rect(3, 0, 1, 3)).copyTo(Tgij);
		Hcij[i](Rect(3, 0, 1, 3)).copyTo(Tcij);

		tempAA = Rgij - eyeM;
		tempbb = Rcg * Tcij - Tgij;

		AA.push_back(tempAA);
		bb.push_back(tempbb);
	}

	invert(AA, pinAA, DECOMP_SVD);
	Tcg = pinAA * bb;

	Rcg.copyTo(Hcg(Rect(0, 0, 3, 3)));
	Tcg.copyTo(Hcg(Rect(3, 0, 1, 3)));

	Hcg.at<double>(3, 0) = 0.0;
	Hcg.at<double>(3, 1) = 0.0;
	Hcg.at<double>(3, 2) = 0.0;
	Hcg.at<double>(3, 3) = 1.0;
}
